#include "JPhysicsCollisionHandler.h"

#include "JPhysObject.h"
/*
// ALWAYS make normalpoint towards object 1
bool JPCollisionHandler::spherePlaneCol(JPhysObject *sphere, JPhysObject *plane, JPVec3 &collisionPoint)
{                                                      
	JPVec3 v = sphere->position() - plane->position();
	
	//v.dot( plane.normal() );
	//float sqrR = sphere.radius()*sphere.radius();

	float distance = v.dot(JPVec3(0.0f,1.0f,0.0f));
	float sqrR = 50.0f*50.0f;

	if(distance*distance <= sqrR)                                      // v.n = |v|cos(theta) since |n|= 1
	{        
		float mtd = sphere.getRadius() - distance;                      // stop the sphere intersecting plane
		JPVector3 mtv = plane.getPlaneNormal() * mtd; 

		sphere.translate(&mtv);                                       // moves spheres by the distance it has intersected

		collisionPoint = sphere.getPosition() - plane.getPlaneNormal() * sphere.getRadius();

		return true;
	}
	return false;                   
}

bool JPCollisionHandler::sphereSphereCol(JPhysicsObject *sphere1, JPhysicsObject *sphere2, JPVector3 &collisionPoint, JPVector3 &n)
{                                                       
	JPVector3 v = *sphere1->getPosition() - *sphere2->getPosition(); // vector between spheres centres

	float sqrDistance = JPMaths::dotProduct(&v, &v);                 // square of the distance between spheres
	float minDistance = sphere1->getRadius() + sphere2->getRadius(); // distance at which spheres are touching

	if(sqrDistance <= minDistance*minDistance)
	{
		float distance = sqrt(sqrDistance);                            // Current distance between the 2 spheres 
		JPVector3 mtd = v * (minDistance - distance) / distance;       // The amount spheres have intersected
		JPVector3 displace = mtd/2.0f;
		sphere1->translate(&displace);                                 // Move spheres to they aren't intersecting
		displace = displace*(-1);
		sphere2->translate(&displace);                                 // (im1 / (im1 + im2)) might be better than just 1/2

		// The unit normal to the collision plane
		n = v/distance;
		// Get the collision normal of unitVector length

		// Point of contact
		collisionPoint = *sphere2->getPosition() + n*sphere2->getRadius();

		return true;
	}
	return false;
}

bool JPCollisionHandler::OBBSphereCol(JPhysicsObject *sphere, JPhysicsObject *box, JPVector3 &collisionPoint, JPVector3 &n)
{
	// Find the point, p on the box closest to the sphere centre
	JPVector3 p = closestPointOBB(box, sphere->getPosition());

	JPVector3 v = *sphere->getPosition() - p; // The vector between sphere centre and closest point on the OBB

	float sqrDistance = JPMaths::dotProduct(&v, &v);        // square of the distance between objects
	float sqrR = sphere->getRadius() * sphere->getRadius(); // sphere radius squared

	if(sqrDistance <= sqrR)
	{
		float distance = sqrt(sqrDistance); // Current distance between the objects
		JPVector3 mtd = v * (sphere->getRadius() - distance) / distance; // The amount objects have intersected
		JPVector3 displace = mtd/2.0f;

		sphere->translate(&displace);       // Move objects so they aren't intersecting   
		displace = displace*(-1.0f);
		box->translate(&displace);          // (im1 / (im1 + im2)) might be better than just 1/2
		collisionPoint = p + displace;

		// The normal to the collision plane
		n = collisionPoint - *sphere->getPosition();
		// Get the collision normal of unit length
		n = JPMaths::unitVector(&n);  

		return true;
	}
	return false;
}

bool JPCollisionHandler::AABBSphereCol(JPhysicsObject *sphere, JPhysicsObject *box,
	JPVector3 &collisionPoint, JPVector3 &n)
{
	// Find the point, p on the box closest to the sphere centre
	JPVector3 p = closestPointAABB(box, sphere->getPosition());

	JPVector3 v = *sphere->getPosition() - p; // The vector between sphere centre and closest point on the OBB

	float sqrDistance = JPMaths::dotProduct(&v, &v);        // square of the distance between objects
	float sqrR = sphere->getRadius() * sphere->getRadius(); // sphere radius squared

	if(sqrDistance <= sqrR)
	{
		float distance = sqrt(sqrDistance); // Current distance between the objects
		JPVector3 mtd = v * (sphere->getRadius() - distance) / distance; // The amount objects have intersected
		JPVector3 displace = mtd/2.0f;

		sphere->translate(&displace);       // Move objects so they aren't intersecting   
		displace = displace*(-1.0f);
		box->translate(&displace);          // (im1 / (im1 + im2)) might be better than just 1/2
		collisionPoint = p + displace;

		// The normal to the collision plane
		n = collisionPoint - *sphere->getPosition();
		// Get the collision normal of unit length
		n = JPMaths::unitVector(&n);  

		return true;
	}

	return false;
}

bool JPCollisionHandler::OBBPlaneCol(JPhysicsObject *box, JPhysicsObject *plane, JPVector3 &collisionPoint)
{
	JPVector3 n = *plane->getPlaneNormal();
	JPVector3 orient[3];
	float r = 0;

	for(int i = 0; i < 3; i++)
	{
		orient[i] = box->getOrientation()[i];
		r += box->getHalfLength()[i]*fabs(JPMaths::dotProduct(&orient[i],&n)); // The extend of the box in the direction of the normal.
	}

	JPVector3 v = *box->getPosition() - *plane->getPosition();

	float distance = JPMaths::dotProduct(&v, &n);

	if(distance*distance <= r*r)
	{
		float mtd = r - distance;  
		JPVector3 mtv = n * mtd; 
		box->translate(&mtv); // moves spheres by the distance it has intersected

		collisionPoint = *box->getPosition() + collisionPointOBB(box->getHalfLength(), orient, &(n*-1.0f));

		return true;
	}
	return false;
}

bool JPCollisionHandler::SAT(JPhysicsObject *box1, JPhysicsObject *box2, JPVector3 &collisionPoint, JPVector3 &n)
{
	float mtdDistance = 1.0E8f;  // make initial depth huge
	float depth;
	JPVector3 mtd, l[15];  // The 15 axis for test

	l[0] = box1->getOrientation()[0];  // Axis paralell to sides of box 1
	l[1] = box1->getOrientation()[1];
	l[2] = box1->getOrientation()[2];

	l[3] = box2->getOrientation()[0];  // Axis paralell to the sides of box2
	l[4] = box2->getOrientation()[1];
	l[5] = box2->getOrientation()[2];

	l[6] = JPMaths::crossProduct(&box1->getOrientation()[0], &box2->getOrientation()[0]);
	l[7] = JPMaths::crossProduct(&box1->getOrientation()[0], &box2->getOrientation()[1]);
	l[8] = JPMaths::crossProduct(&box1->getOrientation()[0], &box2->getOrientation()[2]);

	l[9]  = JPMaths::crossProduct(&box1->getOrientation()[1], &box2->getOrientation()[0]);
	l[10] = JPMaths::crossProduct(&box1->getOrientation()[1], &box2->getOrientation()[1]);
	l[11] = JPMaths::crossProduct(&box1->getOrientation()[1], &box2->getOrientation()[2]);

	l[12] = JPMaths::crossProduct(&box1->getOrientation()[2], &box2->getOrientation()[0]);
	l[13] = JPMaths::crossProduct(&box1->getOrientation()[2], &box2->getOrientation()[1]);
	l[14] = JPMaths::crossProduct(&box1->getOrientation()[2], &box2->getOrientation()[2]);

	for( int i = 0; i < 15; i++)
	{
		if(!testAxis(box1, box2, &l[i], depth)) // If theres a seperation  
		{
			return false;                         // bomb out of the function
		}
		else
		{
			if(depth < mtdDistance)  // Check if intersection of current axis is
			{                        // smaller than current mtd
				mtdDistance = depth;
				mtd = l[i] * mtdDistance;
				n = l[i];
			}
		}
	}
	JPVector3 diff = *box2->getPosition() - *box1->getPosition();

	if (JPMaths::dotProduct(&diff ,&mtd) > 0.0f)
	{
		mtd = mtd*-1.0f;
		n = n*-1.0f;
	}

	box1->translate(&(mtd/2.0f)); 
	box2->translate(&(mtd/-2.0f));

	//Find CollisionPoint
	bool solePoint1 = true;
	bool solePoint2 = true;

	JPVector3 collisionPoint1 = *box2->getPosition() + collisionPointOBBOBB(box2, &n, solePoint1);
	JPVector3 collisionPoint2 = *box1->getPosition() + collisionPointOBBOBB(box1, &(n*-1.0f), solePoint2);

	if(solePoint1)
		collisionPoint = collisionPoint1;
	else if(solePoint2)
		collisionPoint = collisionPoint2;
	else
		collisionPoint = (collisionPoint1 + collisionPoint2) /2;

	return true;
}

bool JPCollisionHandler::AABBPlaneCol(JPhysicsObject *box, JPhysicsObject *plane,
	JPVector3 &collisionPoint)
{ // Essentially the same function as the OBBPlaneCol. Just split up here for clarity.
	// Just using the world x,y,z axis instead of 'box->getOrientation[i]'
	JPVector3 n = *plane->getPlaneNormal();

	JPVector3 orient[3];
	orient[0] = JPVector3(1.0f,0.0f,0.0f);
	orient[1] = JPVector3(0.0f,1.0f,0.0f);
	orient[2] = JPVector3(0.0f,0.0f,1.0f);  

	float r = 0;

	for(int i = 0; i < 3; i++)
	{
		r += box->getAABBHalfLength()[i]*abs(JPMaths::dotProduct(&orient[i],&n)); // The extend of the box in the direction of the normal.
	}

	JPVector3 v = *box->getPosition() - *plane->getPosition();

	float distance = JPMaths::dotProduct(&v, &n);

	if(distance*distance <= r*r)
	{
		float mtd = r - distance;  
		JPVector3 mtv = n * mtd; 
		box->translate(&mtv); // moves spheres by the distance it has intersected

		collisionPoint = *box->getPosition() + collisionPointOBB(box->getAABBHalfLength(), orient, &(n*-1.0f));

		return true;
	}
	return false;
}

bool JPCollisionHandler::AABBAABBCol(JPhysicsObject *box1, JPhysicsObject *box2,
	JPVector3 &collisionPoint, JPVector3 &n)
{ // Stripped down SAT for only first 3 axes as both cubes are aligned to those.

	float mtdDistance = 1.0E8f;  // make initial depth huge
	float depth;
	JPVector3 mtd;  // The 3 axis for test   

	JPVector3 l[3];
	l[0] = JPVector3(1.0f,0.0f,0.0f);
	l[1] = JPVector3(0.0f,1.0f,0.0f);
	l[2] = JPVector3(0.0f,0.0f,1.0f);  

	for( int i = 0; i < 3; i++)
	{
		JPVector3 t = *box1->getPosition() - *box2->getPosition();

		float distance = fabs(JPMaths::dotProduct(&t, &l[i]));

		if(distance > (box1->getAABBHalfLength()[i] + box2->getAABBHalfLength()[i]))
			// If theres a seperation  
			return false;                         // bomb out of the function
		else
		{
			depth = (box1->getAABBHalfLength()[i] + box2->getAABBHalfLength()[i]) - distance;

			if(depth < mtdDistance)  // Check if intersection of current axis is
			{                        // smaller than current mtd
				mtdDistance = depth;
				mtd = l[i] * mtdDistance;
				n = l[i];
			}
		}
	}
	JPVector3 diff = *box2->getPosition() - *box1->getPosition();

	// Always have the normal pointing towards box1
	if (JPMaths::dotProduct(&diff ,&mtd) > 0.0f)  
	{
		mtd = mtd*-1.0f;
		n = n*-1.0f;
	}

	// Intersection Correction
	box1->translate(&(mtd/2.0f));   
	box2->translate(&(mtd/-2.0f));

	//Find CollisionPoint
	JPVector3 collisionPoint1 = *box2->getPosition() + collisionPointAABBAABB(l, box1->getAABBHalfLength(), &n);
	JPVector3 collisionPoint2 = *box1->getPosition() + collisionPointAABBAABB(l, box2->getAABBHalfLength(), &(n*-1.0f));

	collisionPoint = (collisionPoint1 + collisionPoint2) /2; 

	return true;
}

int JPCollisionHandler::objectPlaneResponse(JPhysicsObject *object, JPhysicsObject *plane, JPVector3 *collisionPoint, const float e)
{
	JPVector3 n = *plane->getPlaneNormal();               // collision normal
	JPVector3 R = *collisionPoint - *object->getPosition();// sphere centre to collision point vector
	// Linear velocity at collision point, also the relative velocity as plane.velocity = 0
	JPVector3 v = *object->getLinearVelocity() + JPMaths::crossProduct(object->getAngularVelocity(), &R);
	JPVector3 w;

	float vn = JPMaths::dotProduct(&v, &n);

	if(vn > 0.0f)
	{ 
		return NULL;
	}

	// impulse equation
	JPVector3 rXn = JPMaths::crossProduct(&R, &n)/(*object->getInertiaVec());
	rXn = JPMaths::crossProduct(&rXn, &R);
	float  rXnDn = JPMaths::dotProduct(&rXn, &n); // [((R x n)/I)xR].n

	float j =       -(e+1.0f)*vn /            // j =      -(e+1.0f) * v.n
		(1.0f/object->getMass() + rXnDn); //      1/m + [((R x n)/I)xR].n 
	JPVector3 J = n*j;

	v = *object->getLinearVelocity()  + J/object->getMass(); // v = v + J/m

	w = *object->getAngularVelocity() + JPMaths::crossProduct(&R, &J)/(*object->getInertiaVec()); // w = w + (JxR)/I

	object->setLinearVelocity(&v);
	object->setAngularVelocity(&w);

	return NULL;
}

int JPCollisionHandler::objectObjectResponse(JPhysicsObject *object1, JPhysicsObject *object2, JPVector3 *collisionPoint, JPVector3 *n, const float e)
{
	// Velocities at the point of contact for each sphere
	JPVector3 R1 = *collisionPoint - *object1->getPosition();
	JPVector3 vPoint1 = *object1->getLinearVelocity() + JPMaths::crossProduct(object1->getAngularVelocity(), &R1);

	JPVector3 R2 = *collisionPoint - *object2->getPosition();
	JPVector3 vPoint2 = *object2->getLinearVelocity() + JPMaths::crossProduct(object2->getAngularVelocity(), &R2);

	// impact velocity ( relative velocity )
	JPVector3 v = vPoint1 - vPoint2;

	if(JPMaths::dotProduct(&v,n) > 0.0f)
	{ 
		return NULL;
	}

	// impulse equation
	// 1/m1 + 1/m2
	float invMass1 = 1.0f/object1->getMass();
	float invMass2 = 1.0f/object2->getMass();
	// ((R1 x n)/I1)xR1 
	JPVector3 r1Xn = JPMaths::crossProduct(&R1, n)/(*object1->getInertiaVec());
	r1Xn = JPMaths::crossProduct(&r1Xn, &R1);
	// ((R2 x n)/I2)xR2
	JPVector3 r2Xn = JPMaths::crossProduct(&R2, n)/(*object2->getInertiaVec());
	r2Xn = JPMaths::crossProduct(&r2Xn, &R2); 

	float  rXnDn = JPMaths::dotProduct(&(r1Xn + r2Xn), n);  // ''(R x normal).normal''

	float j = -(e+1.0f)*JPMaths::dotProduct(&v, n) /  // Numerator                     -(e+1.0f) * v.n
		(invMass1 + invMass2 + rXnDn);          // Denominator 1/m1 + 1/m2[((R1 x n)/I1)xR1 + ((R1 x n)/I1)xR1].n

	JPVector3 J = *n * j; // impulse J vector

	// resultant velocities
	JPVector3 velocity1 = *object1->getLinearVelocity() + J * invMass1;  // (v = u + J/m)
	JPVector3 velocity2 = *object2->getLinearVelocity() - J * invMass2;  // (v = u - J/m)

	object1->setLinearVelocity(&velocity1);     
	object2->setLinearVelocity(&velocity2);

	JPVector3 w1 = *object1->getAngularVelocity() + JPMaths::crossProduct(&R1, &J)/(*object1->getInertiaVec()); // w = w + (JxR)/I
	JPVector3 w2 = *object2->getAngularVelocity() - JPMaths::crossProduct(&R2, &J)/(*object2->getInertiaVec()); // w = w + (JxR)/I

	object1->setAngularVelocity(&w1);
	object2->setAngularVelocity(&w2);

	return NULL;
}

JPVector3 JPCollisionHandler::collisionPointOBB(float *halfLength, JPVector3 *orientation, JPVector3 *axis)
{
	float s = 0.0f;
	JPVector3 point(0.0f,0.0f,0.0f);

	for(int i = 0; i < 3; i++)
	{
		s = JPMaths::dotProduct(&orientation[i] , axis);

		if(s < (-EPSILON))
			s = -1.0f;
		else if(s > (EPSILON))
			s = 1.0f;
		else
			s = 0.0f;	

		point = point +	orientation[i] * (s * halfLength[i]);
	}

	return point;
}

JPVector3 JPCollisionHandler::closestPointOBB(JPhysicsObject *box, JPVector3 *p)
{
	JPVector3 v = *p - *box->getPosition();

	JPVector3 point = *box->getPosition(); // Start result at centre of box, make steps from there
	// For each OBB axis..
	for(int i = 0; i < 3; i++)
	{
		// project v onto that axis to get distance
		// along the axis of v from the box centre
		float distance = JPMaths::dotProduct(&v, &box->getOrientation()[i]);
		// If distance is farther than the box extents, clamp to the box
		if(distance > box->getHalfLength()[i])
			distance = box->getHalfLength()[i];
		else if(distance < -box->getHalfLength()[i])
			distance = -box->getHalfLength()[i];
		// Step that distance along the axis to get world coord
		point = point + box->getOrientation()[i]*distance;
	}
	return point;
}

JPVector3 JPCollisionHandler::closestPointAABB(JPhysicsObject *box, JPVector3 *p)
{
	float point[3];
	float centre[3];
	point[0] = p->x;
	point[1] = p->y;
	point[2] = p->z;

	centre[0] = box->getPosition()->x;
	centre[1] = box->getPosition()->y;
	centre[2] = box->getPosition()->z;

	for(int i = 0; i < 3; i++)
	{
		if(point[i] < (centre[i] - box->getAABBHalfLength()[i]))
			point[i] = centre[i] - box->getAABBHalfLength()[i];
		else if(point[i] > (centre[i] + box->getAABBHalfLength()[i]))
			point[i] = centre[i] + box->getAABBHalfLength()[i];
	}
	return JPVector3(point[0],point[1], point[2]);
}

bool JPCollisionHandler::testAxis(JPhysicsObject *box1, JPhysicsObject *box2, JPVector3 *axis, float &depth)
{
	float r1 = 0;
	float r2 = 0;
	JPVector3 t = *box2->getPosition() - *box1->getPosition();

	for(int i = 0; i < 3; i++)
	{
		if(box1->getCollisionType() == OBB)
		{
			r1 += fabs(JPMaths::dotProduct(axis, &box1->getOrientation()[i])) * box1->getHalfLength()[i];
			r2 += fabs(JPMaths::dotProduct(axis, &box2->getOrientation()[i])) * box2->getHalfLength()[i];
		}
		else if(box1->getCollisionType() == AABB)
		{
			r1 += fabs(JPMaths::dotProduct(axis, &box1->getAABBOrientation()[i])) * box1->getAABBHalfLength()[i];
			r2 += fabs(JPMaths::dotProduct(axis, &box2->getAABBOrientation()[i])) * box2->getAABBHalfLength()[i];
		}
	}

	float tDotL = fabs(JPMaths::dotProduct(&t, axis));

	if(tDotL > (r1 + r2)) // Seperation on this axis    
		return false;  

	if(JPMaths::dotProduct(axis, axis) > 0.05f)
		depth = (r1+r2) - tDotL;  // The size of depth along this axis.
	// mtd vector will be depth * axis (depth is magnitude and axis is a unit vector)

	return true;          // Overlap on this axis
}

JPVector3 JPCollisionHandler::collisionPointOBBOBB(JPhysicsObject *box, JPVector3 *axis, bool &singlePoint)
{
	float s = 0.0f;
	JPVector3 point(0.0f,0.0f,0.0f);

	for(int i = 0; i < 3; i++)
	{
		s = JPMaths::dotProduct(&box->getOrientation()[i] , axis);

		if(s < (-EPSILON))
			s = -1.0f;
		else if(s > (EPSILON))
			s = 1.0f;
		else
		{
			s = 0.0f;	
			singlePoint = false;
		}

		point = point +	box->getOrientation()[i] * (s * box->getHalfLength()[i]);
	}

	return point;
}

JPVector3 JPCollisionHandler::collisionPointAABBAABB(JPVector3 *orientation, float *halfLength, JPVector3 *axis)
{
	float s = 0.0f;
	JPVector3 point(0.0f,0.0f,0.0f);

	for(int i = 0; i < 3; i++)
	{
		s = JPMaths::dotProduct(&orientation[i] , axis);

		if(s < (-EPSILON))
			s = -1.0f;
		else if(s > (EPSILON))
			s = 1.0f;
		else
		{
			s = 0.0f;	
		}

		point = point +	orientation[i] * (s * halfLength[i]);
	}

	return point;
}

int JPCollisionHandler::updateDueToGravity(JPhysicsObject *object, float gravity)
{
	if(object->getMass() >= 0.00001f) // If the object is an animate object
	{                                 // boundaries,ground etc dont get effected
		// by gravity
		JPVector3 v;
		v = *object->getLinearVelocity();
		v.y = v.y + gravity;//*time;       // gravity is independant so add it on seperately
		object->setLinearVelocity(&v);  // from other forces
	}
	return NULL;
}
*/
